//
// Created by cyq on 2022/12/9.
//

#include "driver_communicate.h"
#include "driver_chassis.h"
#include "usart.h"
#include "math.h"
RemoteControlStruct remoteControl;
void update_RemoteControlStruct(uint8_t* buffer, RemoteControlStruct *remote_control)
{
    remote_control->rc.ch0 = (buffer[0] | (buffer[1] << 8)) & 0x07ff;
    remote_control->rc.ch1 = ((buffer[1] >> 3) | (buffer[2] << 5)) & 0x07ff;
    remote_control->rc.ch2 = ((buffer[2] >> 6) | (buffer[3] << 2) | (buffer[4] << 10)) & 0x07ff;
    remote_control->rc.ch3 = ((buffer[4] >> 1) | (buffer[5] << 7)) & 0x07ff;
    remote_control->rc.s1 = ((buffer[5] >> 4) & 0x000C) >> 2;
    remote_control->rc.s2 = ((buffer[5] >> 4) & 0x0003);

    uint16_t KeyBoard_value = buffer[6] | (buffer[7] << 8);
    remote_control->key.v = KeyBoard_value;
    remote_control->key.keyW = KEY_PRESSED_OFFSET_W(KeyBoard_value);
    remote_control->key.keyS = KEY_PRESSED_OFFSET_S(KeyBoard_value);
    remote_control->key.keyA = KEY_PRESSED_OFFSET_A(KeyBoard_value);
    remote_control->key.keyD = KEY_PRESSED_OFFSET_D(KeyBoard_value);
    remote_control->key.keyShift = KEY_PRESSED_OFFSET_SHIFT(KeyBoard_value);
    remote_control->key.keyCtrl = KEY_PRESSED_OFFSET_CTRL(KeyBoard_value);
    remote_control->key.keyQ = KEY_PRESSED_OFFSET_Q(KeyBoard_value);
    remote_control->key.keyE = KEY_PRESSED_OFFSET_E(KeyBoard_value);
    remote_control->key.keyR = KEY_PRESSED_OFFSET_R(KeyBoard_value);
    remote_control->key.keyF = KEY_PRESSED_OFFSET_F(KeyBoard_value);
    remote_control->key.keyG = KEY_PRESSED_OFFSET_G(KeyBoard_value);
    remote_control->key.keyZ = KEY_PRESSED_OFFSET_Z(KeyBoard_value);
    remote_control->key.keyX = KEY_PRESSED_OFFSET_X(KeyBoard_value);
    remote_control->key.keyC = KEY_PRESSED_OFFSET_C(KeyBoard_value);
    remote_control->key.keyV = KEY_PRESSED_OFFSET_V(KeyBoard_value);
    remote_control->key.keyB = KEY_PRESSED_OFFSET_B(KeyBoard_value);
}

uint8_t lastWS = 0;
uint8_t resetCount = 0;

//主要是通过遥控器或者按键进行一些模式的控制
void update_gimbalControlData(RemoteControlStruct remote_control, ChassisControlDataStruct *gimbal_control_data)
{
    if (remote_control.rc.s1 == RC_SW_DOWN) {

        gimbal_control_data->speedY = (float)(remote_control.key.keyS - remote_control.key.keyW) * (KEY_MAX_SPEED);

        gimbal_control_data->speedX = (float)(remote_control.key.keyD - remote_control.key.keyA) * (KEY_MAX_SPEED);

        if (remote_control.key.keyZ) {
            gimbal_control_data->chassisMode = CHASSIS_MODE_LOCK;
        }
        else if (remote_control.key.keyE) {
            gimbal_control_data->chassisMode = CHASSIS_MODE_FOLLOW;
        }
        else if (remote_control.key.keyQ) {
            gimbal_control_data->chassisMode = CHASSIS_MODE_GYRO;
        }
        else if(gimbal_control_data->chassisMode == CHASSIS_MODE_AUTOSHOOT) {
            if (remote_control.key.keyCtrl && (remote_control.key.keyC || remote_control.key.keyV))
                gimbal_control_data->chassisMode = CHASSIS_MODE_FOLLOW;
        }
        else if (remote_control.key.keyV) {
            gimbal_control_data->chassisMode = CHASSIS_MODE_AUTOSHOOT;
        }

        if (remote_control.key.keyB) {
            resetCount++;
            if(resetCount > 5)
                gimbal_control_data->resetFlag = 1;
        }
        else {
            resetCount = 0;
            gimbal_control_data->resetFlag = 0;
        }
        if((remote_control.key.keyShift) && (!remote_control.key.keyCtrl))
            gimbal_control_data->superFlag = 1;

        if(remote_control.key.keyShift && remote_control.key.keyCtrl)
            gimbal_control_data->superFlag = 0;
    }
    else {  //遥控器控制
        if (remote_control.rc.s1 == RC_SW_UP){
            gimbal_control_data->chassisMode = CHASSIS_MODE_GYRO;
            gimbal_control_data->superFlag = 0;
//            gimbal_control_data->chassisMode = CHASSIS_MODE_GYRO;
//            gimbal_control_data->superFlag = 0;
        }
        else if (remote_control.rc.s1 == RC_SW_MID) {
            gimbal_control_data->chassisMode = CHASSIS_MODE_FOLLOW;
            gimbal_control_data->superFlag = 0;
        }
        gimbal_control_data->speedX = ((float)(remote_control.rc.ch2 - 1024) / (664.0f)) / (1.0f);
        gimbal_control_data->speedY = (-(float)(remote_control.rc.ch3 - 1024) / (664.0f)) / (1.0f);
    }
}

void GimbalDataUpdate(uint8_t* buffer)
{
    int temp_sum = 0;

    for (uint8_t i = 0; i < 8; i++) {
        temp_sum += buffer[i];
    }
    if (temp_sum != 0)
    {
        if (gimbalControlData.chassisMode == CHASSIS_MODE_FREE) {
            gimbalControlData.chassisMode = CHASSIS_MODE_FOLLOW;
        }
    }
    else {
        //It turns out that gimbal LOSTS its data.So change the mode of chassis to FREE.
        gimbalControlData.resetFlag = 0;
        gimbalControlData.chassisMode = CHASSIS_MODE_FREE;
        resetCount = 0;
        return;
    }
    update_RemoteControlStruct(buffer, &remoteControl);
    update_gimbalControlData(remoteControl, &gimbalControlData);

}

//float FourBytesToFloat(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4)
//{
//    int bits = (byte1 << 24) + (byte2 << 16) + (byte3 << 8) + byte4;
//    int s = (bits >> 31) == 0 ? 1 : -1;
//    int e = (bits >> 23) & 0xff;
//    int m = (e == 0) ?
//            (bits & 0x7fffff) << 1 :
//            (bits & 0x7fffff) | 0x800000;
//    return s * m * (float)pow(2, e - 150);
//}

uint8_t nota5 = 0;
uint8_t judgeHeadTail = 0;//0说明这次收到的是head和cmdid，1说明这次收到的是data和crc
uint16_t cmdid;
uint8_t judgeRecBuffer[300] = {};//前七个字节不收a5，从第二个开始，长度为6
/**
 * @brief 这一段没用了
 * @param UartHandle
 */
//void receiveJudgeHead(void)
//{
//    nota5 = 1;
//    HAL_UART_Receive_DMA(&huart5, judgeRecBuffer, 1);
//}

/**
 * @brief 这一段没用了
 * @param UartHandle
 */
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef* UartHandle)
//{
//    if (UartHandle == &huart5)
//    {
//        if (nota5 == 1)
//        {
//            if (judgeRecBuffer[0] == 0xa5)
//            {
//                nota5 = 0;
//                judgeHeadTail = 0;
//                HAL_UART_Receive_DMA(&huart5, judgeRecBuffer, 6);
//            }
//            else
//            {
//                HAL_UART_Receive_DMA(&huart5, judgeRecBuffer, 1);
//            }
//        }
//        else if (judgeHeadTail == 0)
//        {
//
//            cmdid = (judgeRecBuffer[5] << 8) + judgeRecBuffer[4];
//            uint16_t tempRecLen = (judgeRecBuffer[1] << 8) + judgeRecBuffer[0];
//            judgeHeadTail = 1;
//            HAL_UART_Receive_DMA(&huart5, judgeRecBuffer, tempRecLen + 2);
//        }
//        else if (judgeHeadTail == 1)
//        {
//            switch (cmdid)
//            {
//                case 0x202:
//#if USE_JUDGE_OR_INA226
//                    //chassisPowerController.nowPower = FourBytesToFloat(judgeRecBuffer[4], judgeRecBuffer[5], judgeRecBuffer[6], judgeRecBuffer[7]);
//
//					chassisPowerController.nowPower = FourBytesToFloat(judgeRecBuffer[7], judgeRecBuffer[6],
//							judgeRecBuffer[5], judgeRecBuffer[4]);
//#endif
//                    break;
//            }
//            judgeHeadTail = 0;
//            receiveJudgeHead();
//        }
//    }
//}